#!/usr/bin/env python
#  -*-coding:utf8 -*-
 
from multiprocessing.sharedctypes import RawValue
import rospy
import numpy as np
from std_msgs.msg import Int64
from hellobot_msgs.msg import msg_call, msg_sim
from os import stat
import math
import matplotlib.pyplot as plt
import time

old_data = msg_call()
IsCall = False # 新呼叫
TargetPos = np.array([0,0]) # 目标相对robot的位置
IsArrive = False # 是否到达

k = 0.1 # 前视距离系数
Lfc = 0.2 # 前视距离
Kp = 1.0 # 速度P控制器系数
#dt = 0.05 # 时间间隔，单位：s
dt = 1./60. # 时间间隔，单位：s
L = 0.079 # 车辆轴距，单位：m
wheel_radias = 0.055 # 轮半径，单位：m
vbar = 0.4 # 预计车速, 单位：m/s 转速大概可以到770rpm ==> 4.43m/s

rx=0.0
ry=0.0
ryaw=0.0
rv=0.0
rw=0.0
rvr=0.0
rvl=0.0

init_vr=0 
init_vl=0
init_ryaw=0

class VehicleState:
    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, w=0.0, vr=0.0, vl=0.0):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.w = w
        self.vr = vr
        self.vl = vl

class MyPath:
    def __init__(self,x,y):

        n = int((x**2+y**2)**0.5 * 10) # 10cm 取模得到路径插值点数
        #print(n)
        if abs(x) < 0.1:
            #直线差值
            self.cx = np.arange(0, x, n)
            self.cy = np.arange(0, y, n)
        else:
            #二次贝塞尔
            # P0,P1,P2 = np.array([[0,0],[x,0],[x,y]])# 在此处修改坐标
            # P = lambda t:(1-t)**2*P0 + 2*t*(1-t)*P1 + t**2*P2
            # points = np.array([P(t) for t in np.linspace(0,1,n)])

            #三次贝塞尔
            P0,P1,P2,P3 = np.array([[0,0],[x/2,0],[x*3/4,y*3/4],[x,y]])# 在此处修改坐标
            P = lambda t:(1-t)**3*P0 + 3*t*(1-t)**2*P1 + 3*t**2*(1-t)*P2 + t**3*P3
            points = np.array([P(t) for t in np.linspace(0,1,n)])

            self.cx, self.cy = points[:,0], points[:,1]

def update(state, vr, vl):
    # state.x = state.x + state.v * math.cos(state.yaw) * dt
    # state.y = state.y + state.v * math.sin(state.yaw) * dt
    # state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
    # state.v = state.v + a * dt
    dtheta = (vr - vl)*dt/L
    R = L*(vr+vl)/(2*(vr-vl))
    dl = 2*R*np.sin(vbar*dt/(2*R))
    #state.x = state.x + dl * np.cos(dtheta/2)
    #state.y = state.y + dl * np.sin(dtheta/2)
    state.v = (vr + vl)/2
    state.x = state.x + state.v * math.cos(state.yaw) * dt
    state.y = state.y + state.v * math.sin(state.yaw) * dt
    state.yaw = state.yaw - dtheta 
    state.w = (vr-vl)/L
    state.vr = vr
    state.vl = vl

    global rx,ry,ryaw,rv,rw,rvr,rvl
    rx = state.x
    ry = state.y
    ryaw = state.yaw
    rv = state.v
    rw = state.w
    rvr = state.vr
    rvl = state.vl

    # publish the state to the motor 

    #print(dl)
    #print(state.yaw*180/np.pi)

    return state

def PControl(target, current):
    a = Kp * (target - current)

    return a

def calc_target_index(state, cx, cy):
    # 搜索最临近的路点
    dx = [state.x - icx for icx in cx]
    dy = [state.y - icy for icy in cy]
    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
    ind = d.index(min(d)) # 找出最小的d所在的位置
    J = 0.0

    Lf = k * state.v + Lfc

    while Lf > J and (ind + 1) < len(cx):
        dx = cx[ind + 1] - cx[ind]
        dy = cy[ind + 1] - cy[ind]
        J += math.sqrt(dx ** 2 + dy ** 2)
        ind += 1
        #print(J)

    return ind

def pure_pursuit_control(state, cx, cy, pind):
    ind = calc_target_index(state, cx, cy) #搜寻最近的点 ind

    if pind >= ind:
        ind = pind

    if ind < len(cx): #若目标点没有超过范围，去具体坐标赋予 tx，ty用作目标
        tx = cx[ind]
        ty = cy[ind]
    else: #若超过了，把最后一个点赋给目标
        tx = cx[-1]
        ty = cy[-1]
        ind = len(cx) - 1
        
    alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw #计算当前点到目标点的方向角差
    
    if state.v < 0: # back
        alpha = math.pi - alpha

    Lf = k * state.v + Lfc #前视距离

    if len(cx) < 10:
        vbar_p = vbar/4
    elif ind < 6:
        if (abs(init_vl)<vbar/2) and (abs(init_vr)<vbar/2):
            vbar_p = vbar/(6-ind)
        else:
            vbar_p = vbar
        #vbar_p = vbar/(6-ind)
    elif ind > (len(cx)-4):
        vbar_p = vbar/(ind-len(cx)+4)
    else:
        vbar_p = vbar
    # 前视距离的选取与速度有关，也与单位时间距离有关
    # delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0) #计算转向角
    ld = ((ty-state.y)**2+(tx-state.x)**2)**0.5
    vr = vbar_p - L*vbar_p*np.sin(alpha)/(2*ld)
    vl = 2*vbar_p-vr
    
    return vr, vl, ind

def ReadSpeed(data):
    global init_vr, init_vl, ryaw
    init_vr = data.vr
    init_vl = data.vl
    ryaw = data.rgamma - init_ryaw

def generate(tx,ty):
    # 生成目标路点
    
    path = MyPath(tx,ty)

    #init_vr, init_vl = ReadSpeed()
    # 设置车辆的初始状态
    global TargetPos, IsArrive
    if IsCall:
        state = VehicleState(x=-0.0, y=0.0, yaw=0.0, v=0.2, w=0.0, vr=init_vr, vl=init_vl)
        TargetPos[0] = tx
        TargetPos[1] = ty
    else:
        state = VehicleState(x=rx, y=ry, yaw=ryaw, v=rv, w=rw, vr=init_vr, vl=init_vl)

    lastIndex = len(path.cx) - 1

    target_ind = calc_target_index(state, path.cx, path.cy)
    if lastIndex >= target_ind:
        if not(IsArrive):
            tvr, tvl, target_ind = pure_pursuit_control(state, path.cx, path.cy, target_ind)
            state = update(state, tvr, tvl)
            rospy.loginfo("Isarrived:%0.1f, pid:%d / %d, updateSpeed: %3.4f, %3.4f\n",IsArrive,target_ind,lastIndex,tvr,tvl)
            # plt.cla()
            # plt.plot(path.cx, path.cy, ".r", label="course")
            # plt.plot(state.x, state.y, "ob", label="trajectory")
            # plt.plot(path.cx[target_ind], path.cy[target_ind], "go", label="target")
            # plt.axis("equal")
            # plt.grid(True)
            # plt.title("V_r:" + str(state.vr)[:4] + "; V_l:" + str(state.vl)[:4] + "; Omega:" + str(state.w) + "; yaw:" + str(state.yaw/np.pi*180)+ "degree")
            # plt.pause(0.001)
    dis = (state.x-path.cx[lastIndex])**2+(state.y-path.cy[lastIndex])**2
    if dis < 0.02:
        global rvl,rvr
        IsArrive = True
        rospy.loginfo("Isarrived:%0.1f\n",IsArrive)
        rvl = 0
        rvr = 0

def MovementCommand(data):
    # 控制电机运动
    flag = True
    generate(data.x,data.y)

    #high or low the iopin

    return flag


class Movement():
    def __init__(self):
        self.pub = rospy.Publisher('Motor_Speed', msg_sim, queue_size=1)
        rospy.Subscriber('Move', msg_call, self.callbackMove)
        rospy.Subscriber('SIM', msg_sim, self.callbackSim)
        rate = rospy.Rate(60)

    def callbackMove(self, data):
        global TargetPos, IsCall, IsArrive, old_data, rx, ry, ryaw, init_ryaw
        msg = msg_sim()
        if data.newstatus == False:
            print('waiting for calling')
            IsCall = False
            if (TargetPos[0] != 0) or (TargetPos[1] != 0):
                MovementCommand(old_data)
                msg.vr = rvr
                msg.vl = rvl
                self.pub.publish(msg)
                rospy.loginfo("moveNode Position:%0.2f; %0.2f; %0.2f; %0.2f\n",rx,ry,ryaw/np.pi*180,init_ryaw/np.pi*180)
            #do some sparetime moves?
        else:
            dis = (TargetPos[0]-data.x)**2+(TargetPos[1]-data.y)**2
            if dis < 0.2:
                IsCall = False
                MovementCommand(old_data)
                msg.vr = rvr
                msg.vl = rvl
                self.pub.publish(msg)
            else:
                # IsCall = True
                # IsArrive = False
                # old_data = data
                # MovementCommand(data)
                # msg.vr = rvr
                # msg.vl = rvl
                # self.pub.publish(msg)
                IsCall = True
                IsArrive = False
                old_data = data
                #init_ryaw = ryaw
                init_ryaw = data.yaw
                rx=0.0
                ry=0.0
                ryaw=0.0
                MovementCommand(data)
                msg.vr = rvr
                msg.vl = rvl
                self.pub.publish(msg)
            rospy.loginfo("is a new target: %3.4f ; the target: %3.4f %3.4f\n",IsCall,data.x,data.y)

        #print('remaining: {:.1f}%'.format(data.remain))
        #rospy.loginfo("motor is running")
    def callbackSim(self,data):
        ReadSpeed(data)
        rospy.loginfo("moveNode read vl: %3.4f ; vr: %3.4f\n",init_vl, init_vr)

if __name__ == '__main__':
    rospy.init_node('Motor')
    try:
        Movement()
    except rospy.ROSInterruptException:
        pass
    rospy.spin()
